COMMENT ⊗   VALID 00002 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	! CREATED DEC 30, 1977  ALL PNT,MSM CHANGED TO PNT,HE 
C00007 ENDMK
C⊗;
! CREATED DEC 30, 1977  ALL PNT,MSM CHANGED TO PNT,HE ;

REQUIRE "TLKF5B[PNT,HE]" LOAD_MODULE;
	! NOTE THAT TLKEF5[PNT,MSM] IS NOT THE SAME AS TLKEF5[11,BES];


EXTERNAL INTEGER PROCEDURE TLKEF5(REAL ARRAY TNS,ANGLES);
	! fills TNS with transpose(<arm trans>), ANGLES with
	  joint angles (ANGLES[7]=hand) in degrees & inches.
	  Returns 0 if all ok, otherwise returns 1.

	MAY 17, 1977 - SEEMS TO FILL TNS WITH 3*4 MATRIX;
	;

PRESET_WITH
	    "ELF IS BUSY",
	"ELF STATUS WORD INDICATES ERROR CONDITION",
	"UNABLE TO SERO MASTER NUMBER IN ELF",
	"ELF NOT TRANSFERRING ARM POSITION",
	"BAD READ FROM ELF DURING POSITION DATA TRANSFER",
	"IOPDLOV WHEN TRYING TO SAVE CHANNEL 1 FOR ELF";
INTERNAL STRING ARRAY ARMERR[1:6];

PROCEDURE READ_TRANS(STRING COLOR; REAL ARRAY A);
	BEGIN
	INTEGER I,J;
	OWN REAL ARRAY BESTNS[1:4,1:3], BESANGLES[1:7];
	IF EQU(COLOR,"BLUE")
	THEN BEGIN
		IF (I←TLKEF5(BESTNS,BESANGLES))>0 THEN 
			ABORT1(ARMERR[I],NULL);
		ARRCLR(A);
		FOR I← 1 STEP 1 UNTIL 3
			DO FOR J←1 STEP 1 UNTIL 4
				DO A[I,J]←BESTNS[J,I];
		A[4,4]←1.0;

	        BHAND←BESANGLES[7];	! RETURNS BLUE HAND OPENING;
	     END
	ELSE ABORT1("CANT READ YELLOW ARM YET",NULL);

	END;

PROCEDURE READ_BLUE(REAL ARRAY A);
	READ_TRANS("BLUE", A);

! PROCEDURE READ_YELLOW(REAL ARRAY A);
!	READ_TRANS("YELLOW", A);
IFC NOT DECLARATION(#MOVE) THENC DEFINE #MOVE=TRUE;ENDC

IFC #MOVE THENC

REQUIRE "MOVARM[PNT,HE]" LOAD_MODULE;

EXTERNAL PROCEDURE DR_B_D(INTEGER J; REAL DIF,TT(2.0));
EXTERNAL PROCEDURE DR_B_A(INTEGER J; REAL NEWJ,TT(2.0));
EXTERNAL PROCEDURE DR_Y_D(INTEGER J; REAL DIF,TT(2.0));
EXTERNAL PROCEDURE DR_Y_A(INTEGER J; REAL NEWJ,TT(2.0));
EXTERNAL PROCEDURE MOVE_B(REAL ARRAY A;INTEGER PARK(0);real tt(2.0));
EXTERNAL PROCEDURE MOVE_Y(REAL ARRAY A;INTEGER PARK(0); real tt(2.0));
EXTERNAL PROCEDURE OPNB_D(REAL DIF,TT(2.0));
EXTERNAL PROCEDURE OPNB_A(REAL NEWH,TT(2.0));
EXTERNAL PROCEDURE CENT_B;
EXTERNAL PROCEDURE OPNY_D(REAL DIF,TT(2.0));
EXTERNAL PROCEDURE OPNY_A(REAL NEWH,TT(2.0));
EXTERNAL PROCEDURE CENT_Y;

PROCEDURE OPEN_B_DEL(REAL DIF,TT(2.0));
	OPNB_D(DIF,TT);

PROCEDURE OPEN_B_ABS(REAL NEWH,TT(2.0));
	OPNB_A(NEWH,TT);

PROCEDURE DRIVE_B_DEL(INTEGER J; REAL DIF,TT(2.0));
	DR_B_D(J, DIF,TT);

PROCEDURE DRIVE_B_ABS(INTEGER J; REAL NEWJ,TT(2.0));
	DR_B_A(J,NEWJ,TT);

COMMENT REQUIRE "
WILL REQUIRE FOLLOWING LOAD MODULES DURING LOADING
	TLKF5a[PNT,HE]
	TLKF3a[PNT,HE]
	MOVARM[PNT,HE]
	BEJCZY[PNT,HE]
	ARMSOL[PNT,HE]
" MESSAGE;

ELSEC

PROCEDURE DUMMY;
	PRINT(#VERSION);

DEFINE  DR_B_D(J,D,T)"[][]"=[ DUMMY ],
	DR_B_A(J,D,T)"[][]"=[ DUMMY ],
	DR_Y_D(J,D,T)"[][]"=[ DUMMY ],
	DR_Y_A(J,N,T)"[][]"=[ DUMMY ],
	MOVE_B(J,M,T)"[][]"=[ DUMMY ],
	MOVE_Y(J,M,T)"[][]"=[ DUMMY ],
	OPNB_D(D,T)"[][]"=[ DUMMY ],
	OPNB_A(N,T)"[][]"=[ DUMMY ],
	CENT_B"[][]"=[ DUMMY ],
	OPNY_D(D,T)"[][]"=[ DUMMY ],
	OPNY_A(N,T)"[][]"=[ DUMMY ],
	CENT_Y"[][]"=[ DUMMY ],
	OPEN_B_DEL(D,T)"[][]"=[ DUMMY ],
	OPEN_B_ABS(N,T)"[][]"=[ DUMMY ],
	DRIVE_B_DEL(J,D,F)"[][]"=[ DUMMY ],
	DRIVE_B_ABS(J,D,F)"[][]"=[ DUMMY ];

ENDC